/*
 * FlightController_PID.c
 *
 *  Created on: Jan 21, 2014
 *      Author: ppks
 */

#include "common.h"
#include "FlightController.h"
#include "FlightController_Configuration.h"
#include "FlightController_PID.h"
#include "MathMethods.h"
#include "GPS.h"
#include <avr/pgmspace.h>

/********************************************************************************/
/* Constants Definition                                                         */
/********************************************************************************/
const char PIDnames[] PROGMEM =
  "ROLL;"
  "PITCH;"
  "YAW;"
  "ALT;"
  "Pos;"
  "PosR;"
  "NavR;"
  "LEVEL;"
  "MAG;"
  "VEL;"
;

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
int16_t lastGyro[2] = {0,0};
int16_t errorAngleI[2] = {0,0};

int32_t errorGyroI_YAW;
int16_t delta1[2];
int16_t delta2[2];
int16_t errorGyroI[2] = {0,0};
int32_t prop = 0;

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/


/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/

/*******************************************************************************
* Function Name: FlihgtController_CalculatePID
********************************************************************************
* Summary:
*  This function process PID for PITCH, ROLL and YAW
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FlihgtController_CalculatePID(void)
{
    uint8_t axis;
    int16_t PTerm = 0;
    int16_t ITerm = 0;
    int16_t DTerm;
    int16_t PTermACC;
    int16_t ITermACC;
    int16_t rcSetpoint;
    int16_t error;
    int16_t errorAngle;
    int16_t delta;

    if (globalFlags.HORIZON_MODE )
    {
        prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 512);
    }

    /* Calculate PIDs here for PITCH ROLL */
    for (axis = 0; axis < 2; axis++)
    {
        rcSetpoint = rcCommand[axis] << 1;
        error = rcSetpoint - imuData.gyroData[axis]; /* Calculate error for current axis */

        /* WindUp - limit to 16 here */
        errorGyroI[axis]  = constrain(errorGyroI[axis] + error,-16000,+16000);

        if (abs(imuData.gyroData[axis]) > 640)
        {
            errorGyroI[axis] = 0;
        }

        ITerm = (errorGyroI[axis] >> 7) * FC_Config.PID[axis].I8 >> 6;   /* 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 */

        PTerm = mul(rcSetpoint, FC_Config.PID[axis].P8) >> 6;

        if (globalFlags.ANGLE_MODE || globalFlags.HORIZON_MODE)
        {
            /* axis relying on ACC */
            /* 50 degrees max inclination */
            errorAngle         = constrain(rcSetpoint + GPS_Angle[axis], -500, +500) - att.angle[axis] + FC_Config.RC.angleTrim[axis]; /* 16 bits is ok here */
            errorAngleI[axis]  = constrain(errorAngleI[axis] + errorAngle, -10000, +10000);    /* WindUp  - 16 bits is ok here */

            PTermACC           = mul(errorAngle,FC_Config.PID[PIDLEVEL].P8)>>7; /* 32 bits is needed for calculation: errorAngle*P8 could exceed 32768   16 bits is ok for result */

            int16_t limit      = FC_Config.PID[PIDLEVEL].D8 * 5;
            PTermACC           = constrain(PTermACC, -limit, +limit);

            ITermACC           = mul(errorAngleI[axis], FC_Config.PID[PIDLEVEL].I8) >> 12;   /* 32 bits is needed for calculation:10000*I8 could exceed 32768   16 bits is ok for result */

            ITerm              = ITermACC + ((ITerm-ITermACC) * prop >> 9);
            PTerm              = PTermACC + ((PTerm-PTermACC) * prop >> 9);
        }

        PTerm -= mul(imuData.gyroData[axis], dynP8[axis]) >> 6; /* 32 bits is needed for calculation */

        delta          = imuData.gyroData[axis] - lastGyro[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
        lastGyro[axis] = imuData.gyroData[axis];
        DTerm          = delta1[axis]+delta2[axis]+delta;
        delta2[axis]   = delta1[axis];
        delta1[axis]   = delta;

        DTerm = mul(DTerm, dynD8[axis]) >> 5;        /* 32 bits is needed for calculation */

        axisPID[axis] =  PTerm + ITerm - DTerm;
    }

    /* Calculate PIDs here for YAW */
    #define GYRO_P_MAX  300
    #define GYRO_I_MAX  250

    rcSetpoint = mul(rcCommand[YAW] , (2*FC_Config.RC.yawRate + 30))  >> 5;

    error = rcSetpoint - imuData.gyroData[YAW];
    errorGyroI_YAW  += mul(error, FC_Config.PID[YAW].I8);
    errorGyroI_YAW  = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2 + ((int32_t)1<<28));
    if (abs(rcSetpoint) > 50) errorGyroI_YAW = 0;

    PTerm = mul(error,FC_Config.PID[YAW].P8) >> 6;

#ifndef COPTER_WITH_SERVO
    int16_t limit = GYRO_P_MAX - FC_Config.PID[YAW].D8;
    PTerm = constrain(PTerm, -limit, +limit);
#endif

    ITerm = constrain((int16_t)(errorGyroI_YAW >> 13), -GYRO_I_MAX, +GYRO_I_MAX);

    axisPID[YAW] =  PTerm + ITerm;
}

